import numpy as np

class UAVParameters:
    def __init__(self):
        # 常量
        self.RAD2DEG = 57.2957795
        self.DEG2RAD = 0.0174533
        self.THR_HOVER = 0.609

        # 初始条件
        self.ModelInit_PosE = np.array([0, 0, 0])
        self.ModelInit_VelB = np.array([0, 0, 0])
        self.ModelInit_AngEuler = np.array([0, 0, 0])
        self.ModelInit_RateB = np.array([0, 0, 0])
        self.ModelInit_Rads = 557.142

        # 控制参数
        # 姿态PID参数
        self.Kp_RP_ANGLE = 8.5
        self.Kp_RP_AngleRate = 0.1
        self.Ki_RP_AngleRate = 0.04
        self.Kd_RP_AngleRate = 0.002

        self.Kp_YAW_AngleRate = 0.8
        self.Ki_YAW_AngleRate = 0.02
        self.Kd_YAW_AngleRate = 0.002

        # # 位置PID参数
        # self.Kpxp = 0.75
        # self.Kpyp = 0.75
        # self.Kpzp = 0.6
        # self.Kvxp = 8.5
        # self.Kvxi = 0.60
        # self.Kvxd = 0.005
        # self.Kvyp = 8.5
        # self.Kvyi = 0.60
        # self.Kvyd = 0.008
        # self.Kvzp = 0.45
        # self.Kvzi = 0.01
        # self.Kvzd = 0.008
        # 位置PID参数
        # self.Kpxp = 0.74  # 2.5#0.8
        # self.Kpyp = 0.74  # 2.5#0.8
        # self.Kpzp = 0.80  # 5.5#1.5#0.8
        # self.Kvxp = 9.0  # 5.5#2.5#2.0#1.5
        # self.Kvxi = 0.15  # 0.8#0.5
        # self.Kvxd = 0.4  # 0.6#0.2#0.1
        # self.Kvyp = 9.0  # 5.5#2.5#2.0#1.5
        # self.Kvyi = 0.15  # 0.8#0.5
        # self.Kvyd = 0.4  # 0.6#0.2#0.1
        # self.Kvzp = 9.0
        # self.Kvzi = 0.4
        # self.Kvzd = 0.4
        # self.Kp_RP_ANGLE = 6.5
        # self.Kp_RP_AngleRate = 0.1
        # self.Ki_RP_AngleRate = 0.02
        # self.Kd_RP_AngleRate = 0.001
        #
        # self.Kp_YAW_AngleRate = 0.5
        # self.Ki_YAW_AngleRate = 0.01
        # self.Kd_YAW_AngleRate = 0.00
        #
        # self.Kpxp = 0.7  # 2.5#0.8
        # self.Kpyp = 0.7  # 2.5#0.8
        # self.Kpzp = 0.6  # 5.5#1.5#0.8
        # self.Kvxp = 7.5  # 5.5#2.5#2.0#1.5
        # self.Kvxi = 0.05  # 0.8#0.5
        # self.Kvxd = 1.0  # 0.6#0.2#0.1
        # self.Kvyp = 7.5  # 5.5#2.5#2.0#1.5
        # self.Kvyi = 0.05 # 0.8#0.5
        # self.Kvyd = 1.0  # 0.6#0.2#0.1
        # self.Kvzp = 5.5
        # self.Kvzi = 0.01
        # self.Kvzd = 0.005
        # self.Kp_RP_ANGLE = 6.5
        # self.Kp_RP_AngleRate = 0.1
        # self.Ki_RP_AngleRate = 0.02
        # self.Kd_RP_AngleRate = 0.001
        #
        # self.Kp_YAW_AngleRate = 0.5
        # self.Ki_YAW_AngleRate = 0.01
        # self.Kd_YAW_AngleRate = 0.00

        self.Kpxp = 0.74  # 2.5#0.8
        self.Kpyp = 0.74  # 2.5#0.8
        self.Kpzp = 0.6  # 5.5#1.5#0.8
        self.Kvxp = 3.0  # 5.5#2.5#2.0#1.5
        self.Kvxi = 0.1  # 0.8#0.5
        self.Kvxd = 0.4  # 0.6#0.2#0.1
        self.Kvyp = 3.0  # 5.5#2.5#2.0#1.5
        self.Kvyi = 0.1  # 0.8#0.5
        self.Kvyd = 0.4  # 0.6#0.2#0.1
        self.Kvzp = 4.5
        self.Kvzi = 0.01
        self.Kvzd = 0.005
        # 积分饱和
        self.Saturation_I_RP_Max = 0.3
        self.Saturation_I_RP_Min = -0.3
        self.Saturation_I_Y_Max = 0.2
        self.Saturation_I_Y_Min = -0.2

        # 控制约束
        self.MAX_CONTROL_ANGLE_ROLL = 35
        self.MAX_CONTROL_ANGLE_PITCH = 35
        self.MAX_CONTROL_ANGLE_RATE_PITCH = 220
        self.MAX_CONTROL_ANGLE_RATE_ROLL = 220
        self.MAX_CONTROL_ANGLE_RATE_Y = 200

        # 无人机模型参数
        self.ModelParam_uavMass = 1.4
        self.ModelParam_uavJxx = 0.0232#0.0211
        self.ModelParam_uavJyy = 0.0232#0.0219
        self.ModelParam_uavJzz = 0.0400#0.0366
        self.ModelParam_uavJ = np.diag([self.ModelParam_uavJxx, self.ModelParam_uavJyy, self.ModelParam_uavJzz])
        self.ModelParam_uavType = 3
        self.ModelParam_uavMotNumbs = 4
        self.ModelParam_uavR = 0.225
        self.ModelParam_uavCd = 0.073
        self.ModelParam_uavCCm = np.array([0.01, 0.01, 0.0055])

        self.ModelParam_motorCr = 1148
        self.ModelParam_motorWb = -141.4
        self.ModelParam_motorT = 0.02
        self.ModelParam_motorJm = 0.0001287
        self.ModelParam_motorMinThr = 0.05
        self.ModelParam_rotorCm = 1.779e-07
        self.ModelParam_rotorCt = 1.105e-05

        # 环境参数
        self.ModelParam_envGravityAcc = 9.8
        self.ModelParam_envLongitude = 116.259368300000
        self.ModelParam_envLatitude = 40.1540302
        self.ModelParam_GPSLatLong = [self.ModelParam_envLatitude, self.ModelParam_envLongitude]
        self.ModelParam_envAltitude = -41.5260009765625
        self.ModelParam_BusSampleRate = 0.001


if __name__ == '__main__':
    # 示例用法:
    params = UAVParameters()
